#include "FSM_State_Test.h"

/**
 * Constructor for the FSM State that passes in state specific info to
 * the generic FSM State constructor.
 *
 * @param _controlFSMData holds all of the relevant control data
 */
template <typename T>
FSM_State_Test<T>::FSM_State_Test(ControlFSMData<T>* _controlFSMData)
    : FSM_State<T>(_controlFSMData, FSM_StateName::TEST, "Test") {
    // Do nothing
    // Set the pre controls safety checks
    this->checkSafeOrientation = false;
    // Post control safety checks
    this->checkPDesFoot = false;
    this->checkForceFeedForward = false;

    // test
    // LF
    _dogModel[0].OQ_init << length / 2.0, width / 2.0, 0.0;
    _dogModel[0].OP_init << length / 2.0, width / 2.0, high;
    // RF
    _dogModel[1].OQ_init << length / 2.0, -width / 2.0, 0.0;
    _dogModel[1].OP_init << length / 2.0, -width / 2.0, high;
    // LB
    _dogModel[2].OQ_init << -length / 2.0, width / 2.0, 0.0;
    _dogModel[2].OP_init << -length / 2.0, width / 2.0, high;
    // RB
    _dogModel[3].OQ_init << -length / 2.0, -width / 2.0, 0.0;
    _dogModel[3].OP_init << -length / 2.0, -width / 2.0, high;
}

/**
 *功能：进入状态时要执行的行为
 */
template <typename T>
void FSM_State_Test<T>::onEnter() {
    // Default is to not transition
    this->nextStateName = this->stateName;

    // Reset the transition data
    this->transitionData.zero();
}

/**
 * 实现父类虚函数的具体功能
 * Calls the functions to be executed on each control loop iteration.
 */
template <typename T>
void FSM_State_Test<T>::run() {

     auto seResult = this->_data->_stateEstimator->getResult(); 
     Vec3<float> RPY=seResult.rpy;
    // Quat<float> q4=seResult.orientation;
    //   std::cout<<"r:"<<RPY[0]<<std::endl;
     std::cout<<"p:"<<RPY[1]/3.14*180.0<<std::endl;
    //   std::cout<<"y:"<<RPY[2]<<std::endl;

    //cal_hu(-RPY[0],5-RPY[1],-RPY[2]);
    // Do nothing, all commands should begin as zeros
    float progress =   iter * this->_data->controlParameters->controller_dt;
    float target_roll[3] = { 5., 0., -5. };
    float target_pitch[3] = { 8., 0., -8. };
    float  target_yaw[3] = { 5., 0., -5. };
    if (progress < 1) {
        cal_du(progress * target_roll[1] + (1. - progress) * target_roll[1],
            progress * target_pitch[0] + (1. - progress) * target_pitch[1],
            progress * target_yaw[1] + (1. - progress) *  target_yaw[1]);
    } 
    else if(progress<2)
    {
        float temp=progress-1;
        cal_du(temp * target_roll[1] + (1. - temp) * target_roll[1],
            temp * target_pitch[2] + (1. - temp) * target_pitch[0],
            temp * target_yaw[1] + (1. - temp) *  target_yaw[1]);
    }
    else if(progress<3)
    {
        float temp=progress-2;
        cal_du(temp * target_roll[1] + (1. - temp) * target_roll[1],
            temp * target_pitch[1] + (1. - temp) * target_pitch[2],
            temp * target_yaw[1] + (1. - temp) *  target_yaw[1]);
    }
    else
    {
        iter=0;
    }

    // std::cout << _dogModel[0].QP_Q[0] << "  " << _dogModel[1].QP_Q[0] << "  " << _dogModel[2].QP_Q[0] << "  "
    //           << _dogModel[3].QP_Q[0] << std::endl;
    // std::cout << _dogModel[0].QP_Q[1] << " " << _dogModel[1].QP_Q[1] << " " << _dogModel[2].QP_Q[1] << " "
    //           << _dogModel[3].QP_Q[1] << std::endl;
    // std::cout << _dogModel[0].QP_Q[2] << " " << _dogModel[1].QP_Q[2] << " " << _dogModel[2].QP_Q[2] << " "
    //           << _dogModel[3].QP_Q[2] << std::endl;
    // std::cout << "------------------------------------------------" << std::endl;
    for (int leg = 0; leg < 4; leg++) {
        Vec3<float> tempAng;
        //this->_data->_legController->commands[leg].pDes = _dogModel[leg].QP_Q;
        this->_data->_legController->commands[leg].pDes<<0,0,-0.28;
        computeAuxiliaryAngle(*this->_data->_quadruped, this->_data->_legController->commands[leg].pDes, &tempAng);
        this->_data->_legController->commands[leg].qDes[0] = tempAng(0);
        this->_data->_legController->commands[leg].qDes[1] = tempAng(1) + tempAng(2);
        this->_data->_legController->commands[leg].qDes[2] = tempAng(2) - tempAng(1);

        // test
        // this->_data->_legController->commands[leg].kpJoint << 8, 0, 0, 0, 8, 0, 0, 0, 8;
        // this->_data->_legController->commands[leg].kdJoint << 0.3, 0, 0, 0, 0.3, 0, 0, 0, 0.3;
    }
}

/**
 * 功能：状态转移检查：管理哪些状态可以由用户命令或状态事件触发器切换
 * Manages which states can be transitioned into either by the user
 * commands or state event triggers.
 *
 * @return the enumerated FSM state name to transition into
 */
template <typename T>
FSM_StateName FSM_State_Test<T>::checkTransition() {
    this->nextStateName = this->stateName;
    iter++;

    // Switch FSM control mode
    // switch ((int)this->_data->controlParameters->control_mode) {
    //     case K_PASSIVE:    // normal c (0)
    //         // Normal operation for state based transitions
    //         break;

    //     case K_JOINT_PD:
    //         // Requested switch to joint PD control
    //         this->nextStateName = FSM_StateName::JOINT_PD;
    //         break;

    //     case K_STAND_UP:
    //         // Requested switch to joint PD control
    //         this->nextStateName = FSM_StateName::STAND_UP;
    //         break;

    //     case K_RECOVERY_STAND:
    //         // Requested switch to joint PD control
    //         this->nextStateName = FSM_StateName::RECOVERY_STAND;
    //         break;

    //     case K_LOCOMOTION:
    //         // Requested switch to joint PD control
    //         this->nextStateName = FSM_StateName::LOCOMOTION;
    //         break;

    //     case K_FRONTJUMP:
    //         // Requested switch to joint PD control
    //         this->nextStateName = FSM_StateName::FRONTJUMP;
    //         break;

    //     case K_JUMP:
    //         // Requested switch to joint PD control
    //         this->nextStateName = FSM_StateName::JUMP;
    //         break;

    //     default:
    //         std::cout << "[CONTROL FSM] Bad Request: Cannot transition from " << K_PASSIVE << " to "
    //                   << this->_data->controlParameters->control_mode << std::endl;
    // }

    // Get the next state
    return this->nextStateName;
}

/**
 * 功能：状态转移， 处理机器人在状态之间的实际转换
 * Handles the actual transition for the robot between states.
 * Returns true when the transition is completed.
 *
 * @return true if transition is complete
 */
template <typename T>
TransitionData<T> FSM_State_Test<T>::transition() {
    // Finish Transition
    this->transitionData.done = true;

    // Return the transition data to the FSM
    return this->transitionData;
}

/**
 * 在退出状态时清除状态信息。
 * Cleans up the state information on exiting the state.
 */
template <typename T>
void FSM_State_Test<T>::onExit() {
    // Nothing to clean up when exiting
}

template class FSM_State_Test<float>;

template <typename T>
void FSM_State_Test<T>::cal_du(float roll, float pitch, float yaw) {
    roll = roll / 180.0 * 3.14159;
    pitch = pitch / 180.0 * 3.14159;
    yaw = yaw / 180.0 * 3.14159;
    float SR = sin(roll);
    float CR = cos(roll);
    float SP = sin(pitch);
    float CP = cos(pitch);
    float SY = sin(yaw);
    float CY = cos(yaw);
    Mat3<float> RX, RY, RZ;
    RX << 1, 0, 0, 0, CR, -SR, 0, SR, CR;
    RY << CP, 0, -SP, 0, 1, 0, SP, 0, CP;
    RZ << CY, -SY, 0, SY, CY, 0, 0, 0, 1;
    Rot = RX * RY * RZ;

    for (int leg = 0; leg < 4; leg++) {
        _dogModel[leg].OQ_now = Rot * _dogModel[leg].OQ_init;
        _dogModel[leg].QP_O = _dogModel[leg].OP_init - _dogModel[leg].OQ_now;
        _dogModel[leg].QP_Q = Rot.inverse() * _dogModel[leg].QP_O;
        // OQ_now = Rot * OQ_init;
        //     QP_O = OP_init - OQ_now;
        //     QP_Q = Rot.inverse() * QP_O;
        //     leg_x = QP_Q(0, 0);
        //     leg_y = QP_Q(1, 0);
        //     leg_z = QP_Q(2, 0);
    }
}

template <typename T>
void FSM_State_Test<T>::cal_hu(float roll, float pitch, float yaw) {
    float err=0.04;
    if(roll<err&&roll>-err) roll=0;
    if(pitch<err&&pitch>-err) pitch=0;
    if(yaw<err&&yaw>-err) yaw=0;
    float SR = sin(roll);
    float CR = cos(roll);
    float SP = sin(pitch);
    float CP = cos(pitch);
    float SY = sin(yaw);
    float CY = cos(yaw);
    Mat3<float> RX, RY, RZ;
    RX << 1, 0, 0, 0, CR, -SR, 0, SR, CR;
    RY << CP, 0, -SP, 0, 1, 0, SP, 0, CP;
    RZ << CY, -SY, 0, SY, CY, 0, 0, 0, 1;
    Rot = RX * RY * RZ;

    for (int leg = 0; leg < 4; leg++) {
        _dogModel[leg].OQ_now = Rot * _dogModel[leg].OQ_init;
        _dogModel[leg].QP_O = _dogModel[leg].OP_init - _dogModel[leg].OQ_now;
        _dogModel[leg].QP_Q = Rot.inverse() * _dogModel[leg].QP_O;
    }
}